#include "Main.h"

void main ( void )
{
      float main_batt_soc = 0.0; // State of charge of the main battery, as read by the Cortex
      unsigned char bumper_1_state = 0; 
      unsigned char bumper_2_state = 0; 
      unsigned int jiffies = 0; 

      // Check the battery: 
      main_batt_soc = GetMainBattery ( ) ;
      PrintToScreen ( "Main battery SoC: %d\n" , main_batt_soc ) ;
      // Drive forward: 
      SetMotor ( 1 , 32 ) ;
      SetMotor ( 10 , -32 ) ;
      for ( jiffies = 0 ; jiffies < 1750 ; jiffies++ )
      {
            bumper_1_state = GetDigitalInput ( 1 ) ;
            if ( bumper_1_state == 0 )
            {
                  break ;
            }
            Wait ( 1 ) ;
      }
      // Pause: 
      SetMotor ( 1 , 0 ) ;
      SetMotor ( 10 , 0 ) ;
      Wait ( 500 ) ;
      // Drive backward: 
      SetMotor ( 1 , -32 ) ;
      SetMotor ( 10 , 32 ) ;
      for ( jiffies = 0 ; jiffies < 1750 ; jiffies++ )
      {
            bumper_2_state = GetDigitalInput ( 2 ) ;
            if ( bumper_2_state == 0 )
            {
                  break ;
            }
            Wait ( 1 ) ;
      }
      // Pause: 
      SetMotor ( 1 , 0 ) ;
      SetMotor ( 10 , 0 ) ;
      Wait ( 500 ) ;
      // Turn CCW: 
      SetMotor ( 1 , -64 ) ;
      SetMotor ( 10 , -64 ) ;
      for ( jiffies = 0 ; jiffies < 800 ; jiffies++ )
      {
            bumper_1_state = GetDigitalInput ( 1 ) ;
            bumper_2_state = GetDigitalInput ( 2 ) ;
            if ( bumper_1_state == 0 || bumper_2_state == 0 )
            {
                  break ;
            }
            Wait ( 1 ) ;
      }
      // Stop: 
      SetMotor ( 1 , 0 ) ;
      SetMotor ( 10 , 0 ) ;
}

